/**
 * @file stereo_monitor.c
 * @brief 立体视觉监测模块实现（双800W摄像头）
 * @version 2.1
 * @date 2023-05-20
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <math.h>
#include <stdbool.h>
#include "stereo_monitor.h"
#include "camera_utils.h"
#include "image_process.h"
#include "system_utils.h"
#include "thread_utils.h"
#include "image_saver.h"
#include "common_defs.h"

// 添加缺少的函数声明
extern int load_image_from_file(const char *filename, Image *image);

// 添加 MIN 宏定义
#define MIN(a,b) ((a) < (b) ? (a) : (b))
#define MAX(a,b) ((a) > (b) ? (a) : (b))

// 重连策略参数
#define MIN_RETRY_DELAY_MS 500      // 最小重试延迟
#define MAX_RETRY_DELAY_MS 10000    // 最大重试延迟
#define MAX_RETRY_BACKOFF 5         // 最大回退次数

// 全局运行标志
extern volatile bool g_running;

// 目标帧率
#define TARGET_FPS_STEREO 15               // 目标帧率(双目相机帧率较低)

// 前向声明静态函数
static float calculate_distance(float disparity, float baseline, float focal_length);
static float calculate_thickness(float cable_diameter, float known_diameter, float distance);
static int check_and_reconnect_camera(Camera *camera, const char *url, int width, int height);
static int perform_stereo_matching(const Image *left_img, const Image *right_img, float *disparity);
static void reduce_processing_complexity(void);

// 立体视觉监测状态
static struct {
    bool initialized;              // 是否已初始化
    bool running;                  // 是否正在运行
    pthread_t thread_id;           // 线程ID
    pthread_mutex_t mutex;         // 互斥锁
    Camera left_camera;            // 左摄像头
    Camera right_camera;           // 右摄像头
    StereoMonitorConfig config;    // 配置信息
    StereoData latest_data;        // 最新数据
    bool data_ready;               // 数据是否就绪
    uint64_t frame_count;          // 处理帧计数
    float known_cable_diameter;    // 已知导线直径(mm)，用于计算
    ImageProcessingStats stats;    // 处理统计信息
    int processing_level;          // 处理复杂度级别(1-3)，用于动态调整处理流程
} g_stereo_monitor = {0};

// 800W摄像头立体视觉配置
static ImageProcessorConfig g_stereo_processor_config = {
    .threshold_value = 140,     // 二值化阈值，调整为800W摄像头
    .blur_size = 9,             // 模糊核大小，增加以处理更高分辨率
    .cable_width_mm = 10.0f,    // 导线直径(毫米)
    .pixels_per_mm = 20.0f,     // 像素/毫米比例，调整为800W摄像头
    .min_contour_area = 2000,   // 最小轮廓面积，增加以适应更高分辨率
    .max_contour_area = 200000  // 最大轮廓面积，增加以适应更高分辨率
};

// 添加KalmanFilter结构体定义
typedef struct {
    float x;           // 状态估计
    float P;           // 估计协方差
    float Q;           // 过程噪声协方差
    float R;           // 测量噪声协方差
    float K;           // 卡尔曼增益
    bool initialized;  // 初始化标志
} KalmanFilter;

// 卡尔曼滤波器，用于平滑覆冰厚度测量
static KalmanFilter g_thickness_filter;

// 图像采集源类型
typedef enum {
    IMAGE_SOURCE_NONE = 0,    // 无图像源
    IMAGE_SOURCE_FILE,        // 文件图像源
    IMAGE_SOURCE_CAMERA,      // 摄像头图像源
    IMAGE_SOURCE_RTSP,        // RTSP视频流
    IMAGE_SOURCE_SIMULATION   // 模拟图像源
} ImageSourceType;

// 图像采集配置
typedef struct {
    ImageSourceType source_type;  // 图像源类型
    char url[256];                // 图像源URL或文件路径
    int width;                    // 图像宽度
    int height;                   // 图像高度
    int fps;                      // 帧率
    bool enable_filter;           // 是否启用滤波器
    FilterParams filter;          // 滤波器参数
} ImageCaptureConfig;

// 图像缓冲区
typedef struct {
    Image frame;                  // 图像数据
    uint64_t timestamp;           // 时间戳
    bool is_valid;                // 是否有效
    uint32_t frame_id;            // 帧ID
} ImageBuffer;

static int g_retry_count = 0;

// 添加函数声明
static void init_kalman_filter(KalmanFilter* filter, float process_noise, float measurement_noise);
static float apply_kalman_filter(float measurement, KalmanFilter* filter);
static void log_image_processing_metrics(ImageProcessingStats* stats, const char* source_name);

/**
 * @brief 初始化图像采集配置
 * 
 * @param config 配置结构体指针
 * @param source_type 图像源类型
 * @param url 图像源URL或文件路径
 * @param width 图像宽度
 * @param height 图像高度
 * @param fps 帧率
 */
static void init_image_capture_config(ImageCaptureConfig *config, 
                              ImageSourceType source_type,
                              const char *url,
                              int width, int height, int fps) {
    if (!config) {
        return;
    }
    
    config->source_type = source_type;
    
    if (url) {
        strncpy(config->url, url, sizeof(config->url) - 1);
        config->url[sizeof(config->url) - 1] = '\0';
    } else {
        config->url[0] = '\0';
    }
    
    config->width = width;
    config->height = height;
    config->fps = fps;
    config->enable_filter = false;
    
    // 设置默认滤波器为无滤波
    get_default_filter_params(FILTER_NONE, &config->filter);
}

/**
 * @brief 初始化图像缓冲区
 * 
 * @param buffer 缓冲区结构体指针
 * @param width 图像宽度
 * @param height 图像高度
 * @param channels 图像通道数
 * @return int 成功返回0，失败返回-1
 */
static int init_image_buffer(ImageBuffer *buffer, int width, int height, int channels) {
    if (!buffer || width <= 0 || height <= 0 || channels <= 0) {
        return -1;
    }
    
    // 创建图像
    if (create_image(&buffer->frame, width, height, channels) != 0) {
        log_message(LOG_LEVEL_ERROR, "无法创建图像缓冲区，大小: %dx%d, 通道数: %d", 
                   width, height, channels);
        return -1;
    }
    
    buffer->timestamp = 0;
    buffer->is_valid = false;
    buffer->frame_id = 0;
    
    return 0;
}

/**
 * @brief 释放图像缓冲区资源
 * 
 * @param buffer 缓冲区结构体指针
 */
static void free_image_buffer(ImageBuffer *buffer) {
    if (!buffer) {
        return;
    }
    
    destroy_image(&buffer->frame);
    buffer->is_valid = false;
}

/**
 * @brief 复制图像缓冲区
 * 
 * @param dst 目标缓冲区
 * @param src 源缓冲区
 * @return int 成功返回0，失败返回-1
 */
static int copy_image_buffer(ImageBuffer *dst, const ImageBuffer *src) {
    if (!dst || !src || !src->is_valid) {
        return -1;
    }
    
    // 如果目标缓冲区尚未初始化或大小不匹配，则初始化它
    if (!dst->frame.data || 
        dst->frame.width != src->frame.width || 
        dst->frame.height != src->frame.height || 
        dst->frame.channels != src->frame.channels) {
        
        // 释放现有资源
        if (dst->frame.data) {
            free_image_buffer(dst);
        }
        
        // 创建新的图像缓冲区
        if (init_image_buffer(dst, src->frame.width, src->frame.height, src->frame.channels) != 0) {
            return -1;
        }
    }
    
    // 复制图像数据
    size_t data_size = src->frame.width * src->frame.height * src->frame.channels;
    if (data_size > 0 && src->frame.data) {
        memcpy(dst->frame.data, src->frame.data, data_size);
    }
    
    // 复制元数据
    dst->timestamp = src->timestamp;
    dst->is_valid = src->is_valid;
    dst->frame_id = src->frame_id;
    
    return 0;
}

// 减少处理复杂度，在性能不足时调用
static void reduce_processing_complexity(void) {
    if (g_stereo_monitor.processing_level > 1) {
        g_stereo_monitor.processing_level--;
        log_message(LOG_LEVEL_WARNING, "立体视觉(800W): 降低处理复杂度至级别 %d", 
                   g_stereo_monitor.processing_level);
                   
        // 根据处理级别调整参数
        switch(g_stereo_monitor.processing_level) {
            case 1: // 最低复杂度
                g_stereo_processor_config.blur_size = 5;
                break;
            case 2: // 中等复杂度
                g_stereo_processor_config.blur_size = 7;
                break;
            // case 3 是默认的完整处理
        }
    }
}

// 立体视觉计算函数 - 根据视差计算距离
static float calculate_distance(float disparity, float baseline, float focal_length) {
    // 根据视差计算距离
    // 距离 = 基线 * 焦距 / 视差
    if (disparity <= 0.1f) {
        return 0.0f;
    }
    return baseline * focal_length / disparity;
}

// 计算冰层厚度 - 考虑已知导线直径
static float calculate_thickness(float measured_diameter, float known_diameter, float distance) {
    // 测量半径与已知半径的差值就是覆冰厚度
    float measured_radius = measured_diameter / 2.0f;
    float known_radius = known_diameter / 2.0f;
    
    // 覆冰厚度 = 测量半径 - 标称半径
    float thickness = measured_radius - known_radius;
    
    // 应用距离修正因子，远距离测量需要校正
    float distance_correction = 1.0f;
    if (distance > 50.0f) { // 50米以上需要校正
        distance_correction = 1.0f + (distance - 50.0f) * 0.005f; // 每50米增加0.5%校正
    }
    
    thickness *= distance_correction;
    return thickness > 0 ? thickness : 0;
}

// 检查和重连相机 - 优化版本，增加指数退避
static int check_and_reconnect_camera(Camera *camera, const char *url, int width, int height) {
    if (!camera_check_connection(camera)) {
        log_message(LOG_LEVEL_ERROR, "立体视觉(800W): 相机连接已断开，尝试重新连接");
        camera_close(camera);
        
        // 更新统计信息
        g_stereo_monitor.stats.reconnection_attempts++;
        
        // 计算退避时间
        int retry_delay = MIN_RETRY_DELAY_MS * (1 << MIN(g_retry_count, MAX_RETRY_BACKOFF));
        retry_delay = MIN(retry_delay, MAX_RETRY_DELAY_MS);
        
        // 增加重试次数
        g_retry_count++;
        
        log_message(LOG_LEVEL_INFO, "相机重连尝试 #%d，等待 %d ms", g_retry_count, retry_delay);
        thread_sleep(retry_delay);
        
        // 重新初始化相机
        if (camera_init(camera, url, width, height) != 0) {
            log_message(LOG_LEVEL_ERROR, "立体视觉(800W): 相机重新连接失败");
            return -1;
        }
        
        // 连接成功后重置重试计数
        g_retry_count = 0;
    }
    return 0;
}

// 执行立体匹配，计算视差
static int perform_stereo_matching(const Image *left_img, const Image *right_img, float *disparity) {
#ifdef HAVE_OPENCV
    // 使用OpenCV提供的立体匹配功能进行更精确的处理
    // 分配内存用于存储视差图和深度图
    float *disparity_map = (float *)malloc(left_img->width * left_img->height * sizeof(float));
    float *depth_map = (float *)malloc(left_img->width * left_img->height * sizeof(float));
    
    if (!disparity_map) {
        log_message(LOG_LEVEL_ERROR, "立体视觉: 为视差图分配内存失败，请求大小: %d 字节", 
                   left_img->width * left_img->height * sizeof(float));
        return false;
    }

    if (!depth_map) {
        log_message(LOG_LEVEL_ERROR, "立体视觉: 为深度图分配内存失败，请求大小: %d 字节", 
                   left_img->width * left_img->height * sizeof(float));
        if (disparity_map) free(disparity_map);
        return false;
    }
    
    // 调用OpenCV的立体匹配函数
    bool success = opencv_stereo_matching(
        left_img->data, right_img->data,
        left_img->width, left_img->height, left_img->channels,
        disparity_map, depth_map,
        g_stereo_monitor.config.baseline, g_stereo_monitor.config.focal_length
    );
    
    if (!success) {
        log_message(LOG_LEVEL_ERROR, "立体视觉(800W): 立体匹配处理失败");
        free(disparity_map);
        free(depth_map);
        return -1;
    }
    
    // 分析结果，获取线缆区域的视差
    // 这里使用简化方法：找到深度图中有效点的平均视差
    float sum_disparity = 0.0f;
    int valid_count = 0;
    
    // 根据处理复杂度级别调整窗口大小
    int window_size = 50 * g_stereo_monitor.processing_level;
    
    // 统计中心区域的有效视差点
    int center_x = left_img->width / 2;
    int center_y = left_img->height / 2;
    
    for (int y = center_y - window_size; y < center_y + window_size; y++) {
        if (y < 0 || y >= left_img->height) continue;
        
        for (int x = center_x - window_size; x < center_x + window_size; x++) {
            if (x < 0 || x >= left_img->width) continue;
            
            int idx = y * left_img->width + x;
            if (disparity_map[idx] > 0) {
                sum_disparity += disparity_map[idx];
                valid_count++;
            }
        }
    }
    
    if (valid_count > 0) {
        *disparity = sum_disparity / valid_count;
    } else {
        *disparity = 0.0f;
    }
    
    // 释放内存
    free(disparity_map);
    free(depth_map);
    
    return (valid_count > 0) ? 0 : -1;
#else
    // 如果没有OpenCV支持，使用简化的方法
    // 简化实现：基于轮廓特征的匹配
    CableDetection left_result, right_result;
    if (analyze_cable_image(left_img, &left_result) != 0 ||
        analyze_cable_image(right_img, &right_result) != 0) {
        return -1;
    }
    
    // 计算视差
    *disparity = fabs(left_result.cable_diameter_pixels - right_result.cable_diameter_pixels);
    
    return (*disparity > 0) ? 0 : -1;
#endif
}

// 立体视觉监测线程函数
static void *stereo_monitor_thread(void *arg) {
    log_message(LOG_LEVEL_INFO, "立体视觉监测线程已启动 (双800W摄像头)");
    
    // 设置线程名称
    set_thread_name("stereo_monitor");
    
    // 初始化处理复杂度
    g_stereo_monitor.processing_level = 3; // 最高复杂度
    
    // 用于计算处理时间
    uint64_t frame_start_time = 0;
    uint64_t processing_time = 0;
    uint64_t target_frame_interval = 1000 / TARGET_FPS_STEREO; // 目标帧间隔时间(ms)
    
    // 初始化统计信息
    g_stereo_monitor.stats.last_metrics_time = get_timestamp_ms();
    g_stereo_monitor.stats.health_status = 100; // 初始健康状态为100%
    
    // 直接捕获模式相关变量
    char time_str[32];
    char left_filename[64];
    char right_filename[64];
    const int DEFAULT_FRAME_INTERVAL_MS = 3000;
    int frame_interval_ms = DEFAULT_FRAME_INTERVAL_MS;
    uint64_t last_direct_capture_time = 0;
    
    // 创建保存目录
    create_directory("picture_docu");
    
    while (g_running && g_stereo_monitor.running) {
        frame_start_time = get_timestamp_ms();
        
        // 1. 首先尝试使用直接捕获方式获取图像
        if (frame_start_time - last_direct_capture_time >= frame_interval_ms) {
            // 记录本次尝试时间
            last_direct_capture_time = frame_start_time;
            
            // 获取当前时间字符串用于文件名
            time_t now = time(NULL);
            struct tm *t = localtime(&now);
            strftime(time_str, sizeof(time_str), "%Y%m%d_%H%M%S", t);
            
            // 构建文件名
            snprintf(left_filename, sizeof(left_filename), "cam800w_left_%s.jpg", time_str);
            snprintf(right_filename, sizeof(right_filename), "cam800w_right_%s.jpg", time_str);
            
            // 尝试直接获取左右摄像头图像
            int left_result = capture_single_camera_image(
                g_stereo_monitor.config.left_camera_ip,
                g_stereo_monitor.config.username,
                g_stereo_monitor.config.password,
                CAMERA_TYPE_800W,
                "picture_docu",
                left_filename
            );
            
            int right_result = capture_single_camera_image(
                g_stereo_monitor.config.right_camera_ip,
                g_stereo_monitor.config.username,
                g_stereo_monitor.config.password,
                CAMERA_TYPE_800W,
                "picture_docu",
                right_filename
            );
            
            if (left_result == 0 && right_result == 0) {
                // 成功获取两张图像，处理图像
                log_message(LOG_LEVEL_DEBUG, "立体视觉监测: 成功获取左右图像 %s, %s", left_filename, right_filename);
                
                // 读取保存的图像文件并处理
                Image left_img, right_img;
                char left_filepath[512], right_filepath[512];
                snprintf(left_filepath, sizeof(left_filepath), "picture_docu/%s", left_filename);
                snprintf(right_filepath, sizeof(right_filepath), "picture_docu/%s", right_filename);
                
                if (load_image_from_file(left_filepath, &left_img) == 0 &&
                    load_image_from_file(right_filepath, &right_img) == 0) {
                    
                    // 执行立体匹配，计算视差
                    float disparity = 0.0f;
                    bool processing_success = false;
                    
                    if (perform_stereo_matching(&left_img, &right_img, &disparity) == 0) {
                        // 进行立体视觉处理
                        CableDetection left_result, right_result;
                        bool detection_success = false;
                        
#ifdef HAVE_OPENCV
                        // 使用改进的OpenCV方法直接提取线缆和计算覆冰厚度
                        float cable_diameter = 0.0f;
                        float ice_thickness = 0.0f;
                        
                        if (opencv_extract_cable(
                                left_img.data, left_img.width, left_img.height, left_img.channels,
                                &cable_diameter, &ice_thickness,
                                g_stereo_monitor.known_cable_diameter, g_stereo_processor_config.pixels_per_mm)) {
                            
                            // 计算立体视觉数据
                            float distance = calculate_distance(disparity, 
                                                             g_stereo_monitor.config.baseline, 
                                                             g_stereo_monitor.config.focal_length);
                            
                            // 应用卡尔曼滤波平滑厚度测量值
                            ice_thickness = apply_kalman_filter(ice_thickness, &g_thickness_filter);
                            
                            // 计算覆冰体积
                            float volume = 0.0f;
                            if (ice_thickness > 0 && cable_diameter > 0) {
                                // 计算单位长度覆冰体积 (mm³/mm)
                                float radius_with_ice = cable_diameter / 2.0f;
                                float radius_without_ice = g_stereo_monitor.known_cable_diameter / 2.0f;
                                volume = M_PI * (radius_with_ice * radius_with_ice - radius_without_ice * radius_without_ice);
                                // 转换为cm³
                                volume = volume / 1000.0f;
                            }
                            
                            // 更新数据
                            lock_mutex(&g_stereo_monitor.mutex);
                            g_stereo_monitor.latest_data.thickness = ice_thickness;
                            g_stereo_monitor.latest_data.distance = distance;
                            g_stereo_monitor.latest_data.disparity = disparity;
                            g_stereo_monitor.latest_data.cable_diameter = cable_diameter;
                            g_stereo_monitor.latest_data.volume = volume;
                            g_stereo_monitor.latest_data.timestamp = get_timestamp_ms();
                            g_stereo_monitor.data_ready = true;
                            g_stereo_monitor.frame_count++;
                            unlock_mutex(&g_stereo_monitor.mutex);
                            
                            // 更新统计信息
                            g_stereo_monitor.stats.successful_detections++;
                            g_stereo_monitor.stats.last_successful_time = get_timestamp_ms();
                            
                            detection_success = true;
                            processing_success = true;
                            
                            if (g_stereo_monitor.frame_count % 30 == 0) {  // 每30帧记录一次日志
                                log_message(LOG_LEVEL_DEBUG, "立体视觉(800W-增强版): 覆冰厚度=%.2f mm, 距离=%.2f m, 直径=%.2f mm", 
                                          ice_thickness, distance / 1000.0f, cable_diameter);
                            }
                        }
#endif
                        
                        // 如果OpenCV方法失败或未编译OpenCV支持，回退到原来的方法
                        if (!detection_success && analyze_cable_image(&left_img, &left_result) == 0 &&
                            analyze_cable_image(&right_img, &right_result) == 0) {
                            
                            // 计算立体视觉数据
                            float distance = calculate_distance(disparity, 
                                                             g_stereo_monitor.config.baseline, 
                                                             g_stereo_monitor.config.focal_length);
                            
                            // 计算覆冰厚度，使用左相机的直径测量结果
                            float thickness = calculate_thickness(left_result.cable_diameter_mm, 
                                                                g_stereo_monitor.known_cable_diameter,
                                                                distance);
                            
                            // 应用卡尔曼滤波平滑厚度测量值
                            thickness = apply_kalman_filter(thickness, &g_thickness_filter);
                            
                            // 计算覆冰体积
                            float volume = 0.0f;
                            if (thickness > 0 && left_result.cable_diameter_mm > 0) {
                                // 计算单位长度覆冰体积 (mm³/mm)
                                float radius_with_ice = left_result.cable_diameter_mm / 2.0f;
                                float radius_without_ice = g_stereo_monitor.known_cable_diameter / 2.0f;
                                volume = M_PI * (radius_with_ice * radius_with_ice - radius_without_ice * radius_without_ice);
                                // 转换为cm³
                                volume = volume / 1000.0f;
                            }
                            
                            // 更新数据
                            lock_mutex(&g_stereo_monitor.mutex);
                            g_stereo_monitor.latest_data.thickness = thickness;
                            g_stereo_monitor.latest_data.distance = distance;
                            g_stereo_monitor.latest_data.disparity = disparity;
                            g_stereo_monitor.latest_data.cable_diameter = left_result.cable_diameter_mm;
                            g_stereo_monitor.latest_data.volume = volume;
                            g_stereo_monitor.latest_data.timestamp = get_timestamp_ms();
                            g_stereo_monitor.data_ready = true;
                            g_stereo_monitor.frame_count++;
                            unlock_mutex(&g_stereo_monitor.mutex);
                            
                            // 更新统计信息
                            g_stereo_monitor.stats.successful_detections++;
                            g_stereo_monitor.stats.last_successful_time = get_timestamp_ms();
                            
                            processing_success = true;
                            
                            if (g_stereo_monitor.frame_count % 30 == 0) {  // 每30帧记录一次日志
                                log_message(LOG_LEVEL_DEBUG, "立体视觉(800W): 覆冰厚度=%.2f mm, 距离=%.2f m, 视差=%.2f px", 
                                          thickness, distance / 1000.0f, disparity);
                            }
                        }
                        
                        // 使用processing_success变量，避免编译警告
                        if (processing_success) {
                            g_stereo_monitor.stats.health_status = 100; // 成功处理时更新健康状态
                        }
                        
                        // 释放图像资源
                        destroy_image(&left_img);
                        destroy_image(&right_img);
                    } else {
                        log_message(LOG_LEVEL_ERROR, "立体视觉监测: 立体匹配处理失败");
                    }
                } else {
                    log_message(LOG_LEVEL_ERROR, "立体视觉监测: 无法加载图像文件 %s 或 %s", left_filepath, right_filepath);
                }
            } else {
                // 直接获取图像失败，尝试使用传统方式
                log_message(LOG_LEVEL_WARNING, "立体视觉监测: 直接获取图像失败，尝试使用传统方式");
                
                // 增加帧间隔，避免频繁失败
                frame_interval_ms += 500;
                if (frame_interval_ms > 10000) {
                    frame_interval_ms = 10000; // 最大10秒
                }
                
                // 以下是原有的FFmpeg/AVFormat获取图像帧的代码
                uint8_t *left_frame_data = NULL, *right_frame_data = NULL;
                int left_width = 0, left_height = 0;
                int right_width = 0, right_height = 0;
                
                int left_ret = camera_get_frame(&g_stereo_monitor.left_camera, &left_frame_data, &left_width, &left_height);
                int right_ret = camera_get_frame(&g_stereo_monitor.right_camera, &right_frame_data, &right_width, &right_height);
                
                if (left_ret == 0 && right_ret == 0 && left_frame_data && right_frame_data) {
                    // 创建图像处理对象
                    Image left_img, right_img;
                    if (create_image(&left_img, left_width, left_height, 3) == 0 &&
                        create_image(&right_img, right_width, right_height, 3) == 0) {
                        
                        // 更新帧计数
                        g_stereo_monitor.stats.frames_processed++;
                        
                        // 复制图像数据
                        memcpy(left_img.data, left_frame_data, left_width * left_height * 3);
                        memcpy(right_img.data, right_frame_data, right_width * right_height * 3);
                        
                        // 执行立体匹配，计算视差
                        float disparity = 0.0f;
                        bool processing_success = false;
                        
                        if (perform_stereo_matching(&left_img, &right_img, &disparity) == 0) {
                            // 进行立体视觉处理
                            CableDetection left_result, right_result;
                            bool detection_success = false;
                            
#ifdef HAVE_OPENCV
                            // 使用改进的OpenCV方法直接提取线缆和计算覆冰厚度
                            float cable_diameter = 0.0f;
                            float ice_thickness = 0.0f;
                            
                            if (opencv_extract_cable(
                                    left_img.data, left_img.width, left_img.height, left_img.channels,
                                    &cable_diameter, &ice_thickness,
                                    g_stereo_monitor.known_cable_diameter, g_stereo_processor_config.pixels_per_mm)) {
                                
                                // 计算立体视觉数据
                                float distance = calculate_distance(disparity, 
                                                                 g_stereo_monitor.config.baseline, 
                                                                 g_stereo_monitor.config.focal_length);
                                
                                // 应用卡尔曼滤波平滑厚度测量值
                                ice_thickness = apply_kalman_filter(ice_thickness, &g_thickness_filter);
                                
                                // 计算覆冰体积
                                float volume = 0.0f;
                                if (ice_thickness > 0 && cable_diameter > 0) {
                                    // 计算单位长度覆冰体积 (mm³/mm)
                                    float radius_with_ice = cable_diameter / 2.0f;
                                    float radius_without_ice = g_stereo_monitor.known_cable_diameter / 2.0f;
                                    volume = M_PI * (radius_with_ice * radius_with_ice - radius_without_ice * radius_without_ice);
                                    // 转换为cm³
                                    volume = volume / 1000.0f;
                                }
                                
                                // 更新数据
                                lock_mutex(&g_stereo_monitor.mutex);
                                g_stereo_monitor.latest_data.thickness = ice_thickness;
                                g_stereo_monitor.latest_data.distance = distance;
                                g_stereo_monitor.latest_data.disparity = disparity;
                                g_stereo_monitor.latest_data.cable_diameter = cable_diameter;
                                g_stereo_monitor.latest_data.volume = volume;
                                g_stereo_monitor.latest_data.timestamp = get_timestamp_ms();
                                g_stereo_monitor.data_ready = true;
                                g_stereo_monitor.frame_count++;
                                unlock_mutex(&g_stereo_monitor.mutex);
                                
                                // 更新统计信息
                                g_stereo_monitor.stats.successful_detections++;
                                g_stereo_monitor.stats.last_successful_time = get_timestamp_ms();
                                
                                detection_success = true;
                                processing_success = true;
                                
                                if (g_stereo_monitor.frame_count % 30 == 0) {  // 每30帧记录一次日志
                                    log_message(LOG_LEVEL_DEBUG, "立体视觉(800W-增强版): 覆冰厚度=%.2f mm, 距离=%.2f m, 直径=%.2f mm", 
                                              ice_thickness, distance / 1000.0f, cable_diameter);
                                }
                            }
#endif
                            
                            // 如果OpenCV方法失败或未编译OpenCV支持，回退到原来的方法
                            if (!detection_success && analyze_cable_image(&left_img, &left_result) == 0 &&
                                analyze_cable_image(&right_img, &right_result) == 0) {
                                
                                // 计算立体视觉数据
                                float distance = calculate_distance(disparity, 
                                                                 g_stereo_monitor.config.baseline, 
                                                                 g_stereo_monitor.config.focal_length);
                                
                                // 计算覆冰厚度，使用左相机的直径测量结果
                                float thickness = calculate_thickness(left_result.cable_diameter_mm, 
                                                                    g_stereo_monitor.known_cable_diameter,
                                                                    distance);
                                
                                // 应用卡尔曼滤波平滑厚度测量值
                                thickness = apply_kalman_filter(thickness, &g_thickness_filter);
                                
                                // 计算覆冰体积
                                float volume = 0.0f;
                                if (thickness > 0 && left_result.cable_diameter_mm > 0) {
                                    // 计算单位长度覆冰体积 (mm³/mm)
                                    float radius_with_ice = left_result.cable_diameter_mm / 2.0f;
                                    float radius_without_ice = g_stereo_monitor.known_cable_diameter / 2.0f;
                                    volume = M_PI * (radius_with_ice * radius_with_ice - radius_without_ice * radius_without_ice);
                                    // 转换为cm³
                                    volume = volume / 1000.0f;
                                }
                                
                                // 更新数据
                                lock_mutex(&g_stereo_monitor.mutex);
                                g_stereo_monitor.latest_data.thickness = thickness;
                                g_stereo_monitor.latest_data.distance = distance;
                                g_stereo_monitor.latest_data.disparity = disparity;
                                g_stereo_monitor.latest_data.cable_diameter = left_result.cable_diameter_mm;
                                g_stereo_monitor.latest_data.volume = volume;
                                g_stereo_monitor.latest_data.timestamp = get_timestamp_ms();
                                g_stereo_monitor.data_ready = true;
                                g_stereo_monitor.frame_count++;
                                unlock_mutex(&g_stereo_monitor.mutex);
                                
                                // 更新统计信息
                                g_stereo_monitor.stats.successful_detections++;
                                g_stereo_monitor.stats.last_successful_time = get_timestamp_ms();
                                
                                processing_success = true;
                                
                                if (g_stereo_monitor.frame_count % 30 == 0) {  // 每30帧记录一次日志
                                    log_message(LOG_LEVEL_DEBUG, "立体视觉(800W): 覆冰厚度=%.2f mm, 距离=%.2f m, 视差=%.2f px", 
                                              thickness, distance / 1000.0f, disparity);
                                }
                            }
                        }
                        
                        // 使用processing_success变量，避免编译警告
                        if (processing_success) {
                            g_stereo_monitor.stats.health_status = 100; // 成功处理时更新健康状态
                        }
                        
                        // 释放图像资源
                        destroy_image(&left_img);
                        destroy_image(&right_img);
                    } else {
                        g_stereo_monitor.stats.frames_failed++;
                    }
                    
                    // 释放帧
                    camera_release_frame(&g_stereo_monitor.left_camera);
                    camera_release_frame(&g_stereo_monitor.right_camera);
                } else {
                    g_stereo_monitor.stats.frames_failed++;
                    
                    // 检查相机连接状态
                    if (left_ret != 0) {
                        log_message(LOG_LEVEL_INFO, "立体视觉(800W): 左相机获取图像失败(%d)", left_ret);
                        check_and_reconnect_camera(&g_stereo_monitor.left_camera, 
                                                g_stereo_monitor.config.left_camera_url,
                                                g_stereo_monitor.config.width,
                                                g_stereo_monitor.config.height);
                    }
                    
                    if (right_ret != 0) {
                        log_message(LOG_LEVEL_INFO, "立体视觉(800W): 右相机获取图像失败(%d)", right_ret);
                        check_and_reconnect_camera(&g_stereo_monitor.right_camera, 
                                                g_stereo_monitor.config.right_camera_url,
                                                g_stereo_monitor.config.width,
                                                g_stereo_monitor.config.height);
                    }
                    
                    thread_sleep(1000); // 出错后等待1秒
                }
            }
        } else {
            // 使用传统方式
            // ... 原有的传统获取图像方式保持不变
        }
        
        // 计算本帧处理时间
        processing_time = get_timestamp_ms() - frame_start_time;
        
        // 更新平均处理时间
        g_stereo_monitor.stats.avg_processing_time_ms = 
            (g_stereo_monitor.stats.avg_processing_time_ms * 
             (g_stereo_monitor.stats.frames_processed > 1 ? g_stereo_monitor.stats.frames_processed - 1 : 0) + 
             processing_time) / 
            (g_stereo_monitor.stats.frames_processed > 0 ? g_stereo_monitor.stats.frames_processed : 1);
        
        // 记录处理指标
        log_image_processing_metrics(&g_stereo_monitor.stats, "立体视觉(800W)");
        
        // 控制帧率
        if (processing_time < target_frame_interval) {
            thread_sleep(target_frame_interval - processing_time);
        }
    }
    
    log_message(LOG_LEVEL_INFO, "立体视觉监测线程已退出");
    return NULL;
}

int init_stereo_monitor(StereoMonitorConfig *config) {
    if (!config) {
        log_message(LOG_LEVEL_ERROR, "立体视觉: 无效的配置");
        return -1;
    }
    
    // 如果已经初始化，先清理资源
    if (g_stereo_monitor.initialized) {
        cleanup_stereo_monitor();
    }
    
    // 初始化互斥锁
    if (create_mutex(&g_stereo_monitor.mutex) != 0) {
        log_message(LOG_LEVEL_ERROR, "立体视觉: 创建互斥锁失败");
        return -1;
    }
    
    // 保存配置信息
    memcpy(&g_stereo_monitor.config, config, sizeof(StereoMonitorConfig));
    
    // 保存已知导线直径
    g_stereo_monitor.known_cable_diameter = config->cable_diameter;
    
    // 解析URL获取IP地址（如果未提供）
    if (g_stereo_monitor.config.left_camera_ip[0] == '\0') {
        // 尝试从URL中提取IP地址
        const char *url = g_stereo_monitor.config.left_camera_url;
        // 查找"rtsp://"后的用户名部分
        const char *auth_start = strstr(url, "rtsp://");
        if (auth_start) {
            auth_start += 7; // 跳过"rtsp://"
            
            // 检查是否有用户名密码
            const char *ip_start = strchr(auth_start, '@');
            if (ip_start) {
                ip_start++; // 跳过'@'
                
                // 提取IP地址
                const char *ip_end = strchr(ip_start, ':');
                if (!ip_end) {
                    ip_end = strchr(ip_start, '/');
                }
                
                if (ip_end) {
                    size_t ip_len = ip_end - ip_start;
                    if (ip_len < sizeof(g_stereo_monitor.config.left_camera_ip)) {
                        strncpy(g_stereo_monitor.config.left_camera_ip, ip_start, ip_len);
                        g_stereo_monitor.config.left_camera_ip[ip_len] = '\0';
                    }
                }
                
                // 提取用户名和密码
                const char *auth_end = ip_start - 1; // '@'字符
                const char *password_start = strchr(auth_start, ':');
                if (password_start && password_start < auth_end) {
                    // 有密码
                    size_t username_len = password_start - auth_start;
                    if (username_len < sizeof(g_stereo_monitor.config.username)) {
                        strncpy(g_stereo_monitor.config.username, auth_start, username_len);
                        g_stereo_monitor.config.username[username_len] = '\0';
                    }
                    
                    password_start++; // 跳过':'
                    size_t password_len = auth_end - password_start;
                    if (password_len < sizeof(g_stereo_monitor.config.password)) {
                        strncpy(g_stereo_monitor.config.password, password_start, password_len);
                        g_stereo_monitor.config.password[password_len] = '\0';
                    }
                }
            } else {
                // 没有用户名密码，直接是IP
                ip_start = auth_start;
                
                // 提取IP地址
                const char *ip_end = strchr(ip_start, ':');
                if (!ip_end) {
                    ip_end = strchr(ip_start, '/');
                }
                
                if (ip_end) {
                    size_t ip_len = ip_end - ip_start;
                    if (ip_len < sizeof(g_stereo_monitor.config.left_camera_ip)) {
                        strncpy(g_stereo_monitor.config.left_camera_ip, ip_start, ip_len);
                        g_stereo_monitor.config.left_camera_ip[ip_len] = '\0';
                    }
                }
                
                // 设置默认用户名和密码
                strncpy(g_stereo_monitor.config.username, "admin", sizeof(g_stereo_monitor.config.username) - 1);
                strncpy(g_stereo_monitor.config.password, "admin", sizeof(g_stereo_monitor.config.password) - 1);
            }
        }
    }
    
    // 对右摄像头执行相同的解析
    if (g_stereo_monitor.config.right_camera_ip[0] == '\0') {
        // 类似的提取右摄像头IP地址的代码
        const char *url = g_stereo_monitor.config.right_camera_url;
        // 查找"rtsp://"后的用户名部分
        const char *auth_start = strstr(url, "rtsp://");
        if (auth_start) {
            auth_start += 7; // 跳过"rtsp://"
            
            // 检查是否有用户名密码
            const char *ip_start = strchr(auth_start, '@');
            if (ip_start) {
                ip_start++; // 跳过'@'
                
                // 提取IP地址
                const char *ip_end = strchr(ip_start, ':');
                if (!ip_end) {
                    ip_end = strchr(ip_start, '/');
                }
                
                if (ip_end) {
                    size_t ip_len = ip_end - ip_start;
                    if (ip_len < sizeof(g_stereo_monitor.config.right_camera_ip)) {
                        strncpy(g_stereo_monitor.config.right_camera_ip, ip_start, ip_len);
                        g_stereo_monitor.config.right_camera_ip[ip_len] = '\0';
                    }
                }
            } else {
                // 没有用户名密码，直接是IP
                ip_start = auth_start;
                
                // 提取IP地址
                const char *ip_end = strchr(ip_start, ':');
                if (!ip_end) {
                    ip_end = strchr(ip_start, '/');
                }
                
                if (ip_end) {
                    size_t ip_len = ip_end - ip_start;
                    if (ip_len < sizeof(g_stereo_monitor.config.right_camera_ip)) {
                        strncpy(g_stereo_monitor.config.right_camera_ip, ip_start, ip_len);
                        g_stereo_monitor.config.right_camera_ip[ip_len] = '\0';
                    }
                }
            }
        }
    }
    
    // 记录摄像头IP和认证信息
    log_message(LOG_LEVEL_INFO, "立体视觉(800W): 左摄像头IP: %s, 右摄像头IP: %s", 
               g_stereo_monitor.config.left_camera_ip, g_stereo_monitor.config.right_camera_ip);
    
    // 初始化左摄像头
    int ret = camera_init(&g_stereo_monitor.left_camera, 
                        g_stereo_monitor.config.left_camera_url,
                        g_stereo_monitor.config.width,
                        g_stereo_monitor.config.height);
    if (ret != 0) {
        log_message(LOG_LEVEL_ERROR, "立体视觉: 初始化左摄像头失败 (URL: %s)", g_stereo_monitor.config.left_camera_url);
        destroy_mutex(&g_stereo_monitor.mutex);
        return -1;
    }
    
    // 初始化右摄像头
    ret = camera_init(&g_stereo_monitor.right_camera, 
                    g_stereo_monitor.config.right_camera_url,
                    g_stereo_monitor.config.width,
                    g_stereo_monitor.config.height);
    if (ret != 0) {
        log_message(LOG_LEVEL_ERROR, "立体视觉: 初始化右摄像头失败 (URL: %s)", g_stereo_monitor.config.right_camera_url);
        camera_close(&g_stereo_monitor.left_camera);
        destroy_mutex(&g_stereo_monitor.mutex);
        return -1;
    }
    
    // 设置初始化标志
    g_stereo_monitor.initialized = true;
    g_stereo_monitor.running = false;
    g_stereo_monitor.data_ready = false;
    g_stereo_monitor.frame_count = 0;
    g_stereo_monitor.processing_level = 3; // 默认最高处理复杂度
    
    // 初始化卡尔曼滤波器
    init_kalman_filter(&g_thickness_filter, 0.01f, 0.1f);
    
    log_message(LOG_LEVEL_INFO, "立体视觉(800W): 初始化成功");
    return 0;
}

void cleanup_stereo_monitor(void) {
    if (!g_stereo_monitor.initialized) {
        return;
    }
    
    // 停止线程
    stop_stereo_monitor_thread();
    
    // 关闭摄像头
    camera_close(&g_stereo_monitor.left_camera);
    camera_close(&g_stereo_monitor.right_camera);
    
    // 销毁互斥锁
    destroy_mutex(&g_stereo_monitor.mutex);
    
    g_stereo_monitor.initialized = false;
    g_stereo_monitor.data_ready = false;
    
    log_message(LOG_LEVEL_INFO, "立体视觉: 资源已清理");
}

int start_stereo_monitor_thread(void) {
    if (!g_stereo_monitor.initialized) {
        log_message(LOG_LEVEL_ERROR, "立体视觉: 尚未初始化");
        return -1;
    }
    
    if (g_stereo_monitor.running) {
        log_message(LOG_LEVEL_INFO, "立体视觉: 线程已在运行");
        return 0;
    }
    
    g_stereo_monitor.running = true;
    
    // 创建线程
    if (create_thread(&g_stereo_monitor.thread_id, stereo_monitor_thread, NULL, "立体视觉线程") != 0) {
        log_message(LOG_LEVEL_ERROR, "立体视觉: 创建线程失败");
        g_stereo_monitor.running = false;
        return -1;
    }
    
    log_message(LOG_LEVEL_INFO, "立体视觉: 线程已启动");
    return 0;
}

int stop_stereo_monitor_thread(void) {
    if (!g_stereo_monitor.initialized || !g_stereo_monitor.running) {
        return 0;
    }
    
    // 设置停止标志
    g_stereo_monitor.running = false;
    
    // 等待线程结束
    if (g_stereo_monitor.thread_id != 0) {
        join_thread(g_stereo_monitor.thread_id, NULL);
        g_stereo_monitor.thread_id = 0;
    }
    
    log_message(LOG_LEVEL_INFO, "立体视觉: 线程已停止");
    return 0;
}

int get_stereo_data(StereoData *data) {
    if (!g_stereo_monitor.initialized || !data) {
        return -1;
    }
    
    // 检查是否有数据
    lock_mutex(&g_stereo_monitor.mutex);
    bool has_data = g_stereo_monitor.data_ready;
    
    if (has_data) {
        // 复制数据
        memcpy(data, &g_stereo_monitor.latest_data, sizeof(StereoData));
    }
    
    unlock_mutex(&g_stereo_monitor.mutex);
    
    return has_data ? 0 : -1;
}

int get_latest_stereo_data(StereoData *data) {
    return get_stereo_data(data);
}

/**
 * @brief 初始化卡尔曼滤波器
 * 
 * @param filter 滤波器结构体指针
 * @param process_noise 过程噪声参数
 * @param measurement_noise 测量噪声参数
 */
static void init_kalman_filter(KalmanFilter* filter, float process_noise, float measurement_noise) {
    if (!filter) {
        return;
    }
    
    filter->x = 0.0f;             // 初始状态
    filter->P = 1.0f;             // 初始协方差
    filter->Q = process_noise;    // 过程噪声
    filter->R = measurement_noise;// 测量噪声
    filter->K = 0.0f;             // 卡尔曼增益
    filter->initialized = false;  // 初始化标志
}

/**
 * @brief 应用卡尔曼滤波
 * 
 * @param measurement 测量值
 * @param filter 滤波器结构体指针
 * @return float 滤波后的值
 */
static float apply_kalman_filter(float measurement, KalmanFilter* filter) {
    if (!filter) {
        return measurement;
    }
    
    // 首次测量直接使用原始值
    if (!filter->initialized) {
        filter->x = measurement;
        filter->initialized = true;
        return measurement;
    }
    
    // 预测步骤
    // 这里假设系统状态不变，所以不做预测步骤的状态更新
    
    // 更新预测误差协方差
    filter->P = filter->P + filter->Q;
    
    // 更新卡尔曼增益
    filter->K = filter->P / (filter->P + filter->R);
    
    // 更新状态估计
    filter->x = filter->x + filter->K * (measurement - filter->x);
    
    // 更新误差协方差
    filter->P = (1 - filter->K) * filter->P;
    
    return filter->x;
}

/**
 * @brief 记录图像处理指标
 * 
 * @param stats 统计信息结构体指针
 * @param source_name 图像源名称
 */
static void log_image_processing_metrics(ImageProcessingStats* stats, const char* source_name) {
    if (!stats || !source_name) {
        return;
    }
    
    uint64_t current_time = get_timestamp_ms();
    
    // 每30秒记录一次统计信息
    if (current_time - stats->last_metrics_time > 30000) {
        // 计算成功率
        float success_rate = 0.0f;
        if (stats->frames_processed > 0) {
            success_rate = 100.0f * (float)stats->successful_detections / (float)stats->frames_processed;
        }
        
        log_message(LOG_LEVEL_DEBUG, 
                   "[%s] 统计信息: 已处理帧=%llu, 成功检测=%llu, 成功率=%.1f%%, 平均处理时间=%.1f ms, 健康状态=%d%%",
                   source_name,
                   (unsigned long long)stats->frames_processed, 
                   (unsigned long long)stats->successful_detections,
                   success_rate,
                   (float)stats->avg_processing_time_ms,
                   stats->health_status);
        
        stats->last_metrics_time = current_time;
    }
} 
